// #include<iostream>
// #include<opencv2/opencv.hpp>
// #include<string>
// #include<pcl/io/pcd_io.h>
// #include<pcl/common/transforms.h>
// #include<pcl/console/parse.h>
// #include<pcl/visualization/range_image_visualizer.h>
// #include<pcl/common/common_headers.h>
// #include<pcl/visualization/pcl_visualizer.h>
// #include<pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <opencv2/opencv.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
using namespace std;
struct fileArg
{
    cv::Mat extrinsic_mat, camera_mat,dist_coeff; //外参矩阵，内参矩阵，畸变矩阵
    cv::Mat rotate_mat,transform_vec; //旋转矩阵，平移向量
};

struct calcuArg
{
    cv::Mat rotate_mat;
    cv::Mat transform_vec;
    cv::Mat rotate_vec;
};
pcl::PointCloud<pcl::PointXYZRGB>::Ptr  rgb_cloud (new pcl::PointCloud<pcl::PointXYZRGB> );
ros::NodeHandle nh;
image_transport::ImageTransport imageTransport(nh);
image_transport::Publisher image_pub = imageTransport.advertise("/project_pc_image", 20);


void getMatrixFromFile(cv::String filePath, fileArg& filearg, calcuArg& calarg) {    
	    cv::FileStorage fs(filePath, cv::FileStorage::READ); //打开标定结果文件
        if(!fs.isOpened()) cout<< "open failed"<<endl; 
	    fs["CameraExtrinsicMat"] >> filearg.extrinsic_mat; //从文件里读取4x4外参矩阵
	    fs["CameraMat"] >>filearg.camera_mat; //从文件里读取3x3相机内参矩阵
	    fs["DistCoeff"] >> filearg.dist_coeff; //从文件里读取5x1畸变矩阵
	    fs.release(); //关闭文件
        calarg.rotate_mat=cv::Mat(3, 3, cv::DataType<double>::type); // 将旋转矩阵赋值成3x3矩阵
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                calarg.rotate_mat.at<double>(i,j)=filearg.extrinsic_mat.at<double>(j,i); // 取前三行前三列
            }
        }
        
        //cv::transpose( filearg.camera_mat ,filearg.camera_mat);网上说先做转置，但是转了效果不对
        calarg.rotate_vec = cv::Mat(3, 1, cv::DataType<double>::type); 
        cv::Rodrigues(calarg.rotate_mat, calarg.rotate_vec);

        calarg.transform_vec=cv::Mat(3, 1, cv::DataType<double>::type); //将平移向量赋值成3x1矩阵
        calarg.transform_vec.at<double>(0)=filearg.extrinsic_mat.at<double>(1,3);
        calarg.transform_vec.at<double>(1)=filearg.extrinsic_mat.at<double>(2,3);
        calarg.transform_vec.at<double>(2)=-filearg.extrinsic_mat.at<double>(0,3);
}

void projection(const pcl::PointCloud<pcl::PointXYZI>::Ptr&ccloud,cv::Mat&img, const fileArg& filearg, const calcuArg& calarg){    vector<cv::Point3f> points3d; //存储点云点的vcector，必须存储成cv::Point3f格式
    points3d.reserve(ccloud->size()+1); //先给points3d分配足够大的内存空间，避免push_back时频繁复制转移
    cv::Point3f point;
    for(int i=0;i<ccloud->size();i++)
    {
        point.x=ccloud->points[i].x;
        point.y=ccloud->points[i].y;
        point.z=ccloud->points[i].z;
        points3d.push_back(point); //逐个插入
    }
    vector<cv::Point2f> projectedPoints; //该vector用来存储投影过后的二维点，三维点投影后变二维
    
    // 投影核心函数
    // 第一个参数为原始3d点
    // 第二个参数为旋转矩阵
    // 第三个参数为平移向量
    // 第四个参数为相机内参矩阵
    // 第五个参数为投影结果
    // cv::projectPoints(points3d,rotate_mat,transform_vec,camera_mat,dist_coeff,projectedPoints);
    cv::projectPoints(points3d, calarg.rotate_vec,calarg.transform_vec,filearg.camera_mat,filearg.dist_coeff,projectedPoints);
    pcl::PointXYZRGB point_rgb;
	//遍历投影结果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2f p = projectedPoints[i];
        point_rgb.x=ccloud->points[i].x;
        point_rgb.y=ccloud->points[i].y;
        point_rgb.z=ccloud->points[i].z;
        point_rgb.r=0;
        point_rgb.g=0;
        point_rgb.b=0;

		// 由于图像尺寸为480x640，所以投影后坐标不在图像范围内的点不进行染色（默认黑色）
        if (p.y<480&&p.y>=0&&p.x<640&&p.x>=0) 
        {
            point_rgb.r=int(img.at<cv::Vec3b>(p.y,p.x)[2]); //读取像素点的rgb值
            point_rgb.g=int(img.at<cv::Vec3b>(p.y,p.x)[1]);
            point_rgb.b=int(img.at<cv::Vec3b>(p.y,p.x)[0]);
        }
        rgb_cloud->push_back(point_rgb); //对于投影后在图像中的点进行染色后加入点云rgb_cloud
    }
    // sensor_msgs::PointCloud2 ros_cloud; // 申明ros类型点云
    // pcl::toROSMsg(*rgb_cloud,ros_cloud); // pcl点云转ros点云
    // ros_cloud.header.frame_id="rslidar"; //
    // cloud_pub.publish(ros_cloud); //cloud_pub是在函数外定义的一个ros点云发布器，将染色后的点云发布
    
    //再次遍历投影结果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2f p = projectedPoints[i];
        // 由于图像尺寸为720x1280，所以投影后坐标处于图像范围内的点才在图像中进行标示
        if (p.y<720&&p.y>=0&&p.x<1280&&p.x>=0)
        {
        	//标示的方式是在对应位置画圆圈
            cv::circle(img, p, 1, cv::Scalar(0, 0,255 ), 1, 8, 0);
        }
    }
    sensor_msgs::ImagePtr msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",img).toImageMsg(); //利用cv_bridge将opencv图像转为ros图像
    image_pub.publish(msg); //image_pub是在函数外定义的一个ros图像发布器，将标示后的图像发布

}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "fusion_node");
    // ros::NodeHandle nh;
    fileArg fileinfo;
    calcuArg calout;
    // 读取标定参数文件
    std::string calib_file = "/home/songlin/work/calibrate.yaml";  // 替换为实际的参数文件路径
    //获取矩阵信息
    getMatrixFromFile(calib_file, fileinfo, calout);
    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr  rgb_cloud (new pcl::PointCloud<pcl::PointXYZRGB> );
//   CalibrationParams params = readCalibrationParams(calib_file);

//   // 订阅点云数据话题
//   ros::Subscriber point_cloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/point_cloud_topic", 1, boost::bind(pointCloudCallback, _1, params));

//   // 订阅相机图像数据话题
//   ros::Subscriber image_sub = nh.subscribe<sensor_msgs::Image>("/image_topic", 1, boost::bind(imageCallback, _1, params));

//   // 创建新的话题发布器，用于发布投影结果
//   ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("/projected_image_topic", 1);
    // image_transport::ImageTransport imageTransport(nh);
    // image_transport::Publisher image_pub = imageTransport.advertise("/project_pc_image", 20);

//   ros::spin();
    ros::Publisher project_img_pub;
    // image_transport::ImageTransport imageTransport(nh);
    // image_transport::Publisher image_pub = imageTransport.advertise("/project_pc_image", 20);

    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/camera/color/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, "/rslidar_points", 5);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), pcl_sub,image_sub);
    sync.registerCallback(boost::bind(&projection, &fileinfo, &calout, _1, _2));

   
    ros::spin();

  return 0;
}


